﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace GeoFits
{
    //*************************************************************
    // 查找对应点方法描述如下：
    // 1、首先找出找出相同的三个点，具体执行如下
    // a,找相似的两个距离1（相似表示为两个距离差小于DISMINERROR,但与其他距离相差大于DISMAXERROR）
    // b,找出第三点，计算和距离1的两个端点间的距离2，距离3是不是一个没有长度相似的边
    // c,在desDis中找出一条与距离2长度相似的边。
    // d,根据两个条长度相似的边找出三个对应点。
    // e,对第三条边进行较验。
    // 注意：索引+1时会超出
    //**************************************************************

    /// <summary>
    /// 根据距离在初始点和目标点中查找对应点 
    /// </summary>
    /// <remarks>
    /// 此方法可以快速找出最佳拟合中的对应点，但在点非常密集时容易出错或找不到对应点，需要谨慎使用。
    /// </remarks>
    public static class FindPointPairs
    {        
        /// <summary>
        /// 两条边的差值大于此值认为两边完全不相同
        /// </summary>
        /// <value>两条边的差值大于此值认为两边完全不相同</value>
        /// <remarks>推荐值为6倍误差，默认值为0.090</remarks>
        public static double DISMAXERROR = 0.090;
        /// <summary>
        /// 两条边的差值小于此值认为两边完全相同
        /// </summary>
        /// <value>两条边的差值小于此值认为两边完全相同</value>
        /// <remarks>推荐值为3倍误差，默认值为0.045</remarks>
        public static double DISMINERROR = 0.045;

        private static List<KeyValuePair<int, int>> SearchForm0(List<Point> srcList,List<Point> desList)
        {
            List<KeyValuePair<int, int>> pairs = new List<KeyValuePair<int, int>>();
            var flag = Find3PairsFrom0(ref pairs, ref srcList, ref desList);
            if (!flag)
            {
                throw new Exception("无法自动查找对应点！");
            }
            return Search(srcList, desList, pairs);
        }
        //public static List<KeyValuePair<int, int>> Search(List<Point> pts1, List<Point> pts2, List<KeyValuePair<int, int>> prs)
        //{
        //    List<KeyValuePair<int, int>> pairs = new List<KeyValuePair<int, int>>();

        //    return pairs;
        //}
        /// <summary>
        /// 查找对应点
        /// </summary>
        /// <param name="srcList">初始点</param>
        /// <param name="desList">目标点</param>
        /// <param name="pairs">已有点对：不指定或指定为null时完全从0开始。</param>
        /// <returns>找到的点对的序号。</returns>
        public static List<KeyValuePair<int, int>> Search(List<Point> srcList, List<Point> desList,List<KeyValuePair<int,int>> pairs=null)
        {
            if (pairs == null || pairs.Count == 0)
            {
                return SearchForm0(srcList, desList);
            }
            while (Find1PairFrom1More(ref pairs, ref srcList, ref desList)) ;
            return pairs;
        }
        
        /// <summary>
        /// 
        /// </summary>
        /// <param name="pairs">原先有的点对</param>
        /// <param name="srcList">点序列1</param>
        /// <param name="desList">点序列1</param>
        /// <returns>序列1中的点号为key，序列2中的点号为value</returns>
        private static bool Find3PairsFrom0(ref List<KeyValuePair<int, int>> pairs, ref List<Point> srcList, ref List<Point> desList)
        {
            List<double> srcDisList = new List<double>();
            List<KeyValuePair<int, int>> srcDisIndexList = new List<KeyValuePair<int, int>>();
            //按从小到大的顺序排序
            for (int i = 0; i < srcList.Count; i++)
            {
                for (int m = i + 1; m < srcList.Count; m++)
                {
                    double dis = srcList[i].DistanceTo(srcList[m]);
                    int n = 0;
                    for (; n < srcDisList.Count; n++)
                    {
                        if (srcDisList[n] > dis)
                        {
                            break;
                        }
                    }
                    srcDisList.Insert(n, dis);
                    srcDisIndexList.Insert(n, new KeyValuePair<int, int>(i, m));
                }
            }
            List<double> desDisList = new List<double>();
            List<KeyValuePair<int, int>> desDisIndexList = new List<KeyValuePair<int, int>>();
            for (int i = 0; i < desList.Count; i++)
            {
                for (int m = i + 1; m < desList.Count; m++)
                {
                    double dis = desList[i].DistanceTo(desList[m]);
                    int n = 0;
                    for (; n < desDisList.Count; n++)
                    {
                        if (desDisList[n] > dis)
                        {
                            break;
                        }
                    }
                    desDisList.Insert(n, dis);
                    desDisIndexList.Insert(n, new KeyValuePair<int, int>(i, m));
                }
            }   
            //找到一条与其他边相差较大的边
            int desIndex = FindADiffDis(0, desDisList);
            if (desIndex == -1)
            {
                return false;
            }
            while (desIndex < desDisList.Count)
            {
                //在srcDisList中找到一条与这条边基本相等但与srcDisList其他边相差较大的边
                int scrIndex = FindASameDis(desDisList[desIndex], srcDisList);
                if (scrIndex == -1)
                {
                    //没有找到
                    if (desIndex == desDisList.Count - 1)
                    {
                        break;
                    }
                    desIndex = FindADiffDis(desIndex + 1, desDisList);
                    continue;
                }
                //找到相同的边，此时认为两点对应，来查找第三点
                for (int i = 0; i < desDisList.Count; i++)
                {
                    if (i == desDisIndexList[desIndex].Key || i == desDisIndexList[desIndex].Value)
                    {
                        continue;
                    }

                    int ind = desDisIndexList.FindIndex(new KeyValuePair<int, int>(i, desDisIndexList[desIndex].Key));
                    double disIK = desDisList[ind];
                    if ((ind == desDisList.Count - 1) && (disIK - desDisList[ind - 1] < DISMAXERROR))
                    {
                        continue;
                    }
                    if (disIK - desDisList[ind - 1] < DISMAXERROR || desDisList[ind + 1] - disIK < DISMAXERROR)
                    {
                        continue;
                    }
                    //查找disIK，disIV是不是有比较相似的距离
                    ind = desDisIndexList.FindIndex(new KeyValuePair<int, int>(i, desDisIndexList[desIndex].Value));
                    double disIV = desDisList[ind];
                    //查找disIK，disIV是不是有比较相似的距离
                    if ((ind == desDisList.Count - 1) && disIV - desDisList[ind - 1] < DISMAXERROR)
                    {
                        continue;
                    }
                    if (disIV - desDisList[ind - 1] < DISMAXERROR || desDisList[ind + 1] - disIV < DISMAXERROR)
                    {
                        continue;
                    }
                    //然后在srcDisList中找边，先找到与disIK相同的边
                    int thrIndex = FindASameDis(disIK, srcDisList);
                    //此时在完全理想的状态下thrSrc与i，key与
                    if (thrIndex == -1)
                    {
                        //表示没有找到对应的边
                        //desIndex = FindADiffDis(desIndex + 1, desDisList);
                        continue;
                    }
                    //此时找到一个非常好的对应边
                    int ii = srcDisIndexList[thrIndex].Key, kk = srcDisIndexList[scrIndex].Key, vv = srcDisIndexList[scrIndex].Value;
                    if (ii == kk)
                    {
                        //说明ii不对，kk与vv是正确的
                        ii = srcDisIndexList[thrIndex].Value;
                    }
                    else if (ii == vv)
                    {
                        //说明ii不对，kk与vv是也是错误的
                        ii = srcDisIndexList[thrIndex].Value;
                        kk = srcDisIndexList[scrIndex].Value;
                        vv = srcDisIndexList[scrIndex].Key;
                    }
                    else
                    {
                        //说明ii是正确的，此时srcDisIndexList[thrIndex].Value一定要与kk或vv相同
                        int temInd = srcDisIndexList[thrIndex].Value;
                        if (temInd == vv)
                        {
                            //说明vv与kk是错误的
                            kk = srcDisIndexList[scrIndex].Value;
                            vv = srcDisIndexList[scrIndex].Key;
                        }
                        else if (temInd == kk)
                        {
                            //一切都是正常的，不用做任何事
                        }
                        else
                        {
                            //此时表示这个距离是错误的，继续
                            //desIndex = FindADiffDis(desIndex + 1, desDisList);
                            continue;
                        }
                    }

                    //此时已经找到三个点，最终做一次校验
                    int frIndex = FindASameDis(disIV, srcDisList);
                    if (frIndex != desDisIndexList.FindIndex(new KeyValuePair<int, int>(ii, vv)))
                    {
                        //说明最后一次没有成功
                        continue;
                    }

                    //此时找到了三个点，成功
                    pairs.Add(new KeyValuePair<int, int>(ii, i));
                    pairs.Add(new KeyValuePair<int, int>(kk, desDisIndexList[desIndex].Key));
                    pairs.Add(new KeyValuePair<int, int>(vv, desDisIndexList[desIndex].Value));
                    return true;
                }
            }
            return false;
        }

        private static int FindIndex(this List<KeyValuePair<int, int>> ps, KeyValuePair<int, int> p)
        {
            for (int i = 0; i < ps.Count; i++)
            {
                if (ps[i].Key == p.Key && ps[i].Value == p.Value)
                {
                    return i;
                }
                if (ps[i].Key == p.Value && ps[i].Value == p.Key)
                {
                    return i;
                }
            }
            return -1;
        }
        
        private static int FindADiffDis(int startIndex, List<double> desDisList)
        {
            int cou = desDisList.Count;
            if (startIndex == 0 && desDisList[1] - desDisList[0] > DISMAXERROR)
            {
                return 0;
            }
            for (int i = (startIndex == 0 ? 1 : startIndex); i < cou - 1; i++)
            {
                if (desDisList[i] - desDisList[i - 1] > DISMAXERROR && desDisList[i + 1] - desDisList[i] > DISMAXERROR)
                {
                    return i;
                }
            }
            //两种情况1、最后一个数可能是不同的边。2、没有不同的边
            if (desDisList[cou - 1] - desDisList[cou - 2] > DISMAXERROR)
            {
                return cou - 1;
            }
            return -1;
            //throw new Exception("无法找到差别较小的边！");
        }
        /// <summary>
        /// 找到一条相同的边，这条边一个端点在pair中，另一个端点返回
        /// </summary>
        /// <param name="pairs"></param>
        /// <param name="srcDisList"></param>
        /// <param name="srcDisIndexList"></param>
        /// <param name="desDisList"></param>
        /// <param name="desDisIndexList"></param>
        /// <param name="ptCount">des点的个数</param>
        /// <returns>如果没有返回-1，-1</returns>
        private static KeyValuePair<int, int> Find1PairFrom1More(List<KeyValuePair<int, int>> pairs, List<double> srcDisList, List<KeyValuePair<int, int>> srcDisIndexList, List<double> desDisList, List<KeyValuePair<int, int>> desDisIndexList, int ptCount)
        {
            bool newPairFlag = false;
            KeyValuePair<int, int> newFoundPair = new KeyValuePair<int, int>(-1, -1);
            for (int m = 0; m < pairs.Count; m++)
            {
                var item = pairs[m];
                for (int i = 0; i < ptCount; i++)
                {
                    if (NoInListValue(i, pairs))
                    {
                        continue;
                    }

                    if (haveNoSameDist(item.Value, i, desDisList, desDisIndexList))
                    {
                        double desDis = desDisList[desDisIndexList.FindIndex(new KeyValuePair<int, int>(item.Value, i))];
                        //表明没有相似的,再在srcDisList找没有类似的边（以item.key为一个节点）
                        var srcIndex = FindASameDis(desDis, item.Key, srcDisList, srcDisIndexList);
                        if (srcIndex == -1)
                        {
                            continue;
                        }
                        else
                        {
                            //找到一条无相似的边，步骤1完成，此时找到的对应点value是i，key是srcDisIndexList[srcIndex]与已知点不同的
                            var newPair = srcDisIndexList[srcIndex];
                            if (newPair.Key == item.Key)
                            {
                                //说明key不为新发现的点
                                newFoundPair = new KeyValuePair<int, int>(newPair.Value, i);
                                newPairFlag = true;
                            }
                            else if (newPair.Value == item.Key)
                            {
                                //说明value不为新发现的点
                                newFoundPair = new KeyValuePair<int, int>(newPair.Key, i);
                                newPairFlag = true;
                            }
                            else
                            {
                                //说明不符合条件
                                continue;
                            }
                        }
                    }
                    else
                    {
                        //有相似的则到下一点
                        continue;
                    }
                    if (newPairFlag)
                    {
                        foreach (var item2 in pairs)
                        {
                            if (Math.Abs(
                                srcDisList[srcDisIndexList.FindIndex(new KeyValuePair<int, int>(newFoundPair.Key, item2.Key))] -
                                desDisList[desDisIndexList.FindIndex(new KeyValuePair<int, int>(newFoundPair.Value, item2.Value))]
                                ) > DISMINERROR)
                            {
                                newPairFlag = false;
                            }
                        }
                        if (newPairFlag)
                            break;
                        else
                            continue;
                    }
                }                

                if (newPairFlag)
                {
                    return newFoundPair;
                    //break;
                }
            }

            return new KeyValuePair<int, int>(-1, -1);
            //throw new NotImplementedException();
        }
        private static bool Find1PairFrom1More(ref List<KeyValuePair<int, int>> pairs, ref List<Point> srcList, ref List<Point> desList)
        {
            List<double> srcDisList = new List<double>();
            List<KeyValuePair<int, int>> srcDisIndexList = new List<KeyValuePair<int, int>>();
            //按从小到大的顺序排序
            for (int i = 0; i < srcList.Count; i++)
            {
                for (int m = i + 1; m < srcList.Count; m++)
                {
                    double dis = srcList[i].DistanceTo(srcList[m]);
                    int n = 0;
                    for (; n < srcDisList.Count; n++)
                    {
                        if (srcDisList[n] > dis)
                        {
                            break;
                        }
                    }
                    srcDisList.Insert(n, dis);
                    srcDisIndexList.Insert(n, new KeyValuePair<int, int>(i, m));
                }
            }
            List<double> desDisList = new List<double>();
            List<KeyValuePair<int, int>> desDisIndexList = new List<KeyValuePair<int, int>>();
            for (int i = 0; i < desList.Count; i++)
            {
                for (int m = i + 1; m < desList.Count; m++)
                {
                    double dis = desList[i].DistanceTo(desList[m]);
                    int n = 0;
                    for (; n < desDisList.Count; n++)
                    {
                        if (desDisList[n] > dis)
                        {
                            break;
                        }
                    }
                    desDisList.Insert(n, dis);
                    desDisIndexList.Insert(n, new KeyValuePair<int, int>(i, m));
                }
            }
            //方法：//1、查找一条无近似的边（只要到已知点对中任意一对点两条边都是无相似的点就可以）
            KeyValuePair<int, int> newFoundPair = Find1PairFrom1More(pairs, srcDisList, srcDisIndexList, desDisList, desDisIndexList, desList.Count);
            //KeyValuePair<int, int> newFoundPair = new KeyValuePair<int, int>();

            if (newFoundPair.Key != -1 && newFoundPair.Value != -1)
            {
                pairs.Add(newFoundPair);
                return true;
            }
            return false;
        }        
        /// <summary>
        /// 找到一条相同的边，没有限制条件
        /// </summary>
        /// <param name="desDis"></param>
        /// <param name="srcDisList"></param>
        /// <returns>返回点的index，没有则返回-1</returns>
        private static int FindASameDis(double desDis, List<double> srcDisList)
        {
            if (Math.Abs(srcDisList[0] - desDis) < DISMINERROR && (srcDisList[1] - srcDisList[0]) > DISMAXERROR)
            {
                return 0;
            }
            int cou = srcDisList.Count;
            for (int i = 1; i < cou - 1; i++)
            {
                if (Math.Abs(srcDisList[i] - desDis) < DISMINERROR &&
                    (srcDisList[i + 1] - srcDisList[i]) < DISMAXERROR &&
                    (srcDisList[i] - srcDisList[i - 1]) < DISMAXERROR)
                {
                    return i;
                }
            }
            if (Math.Abs(srcDisList[cou - 1] - desDis) < DISMINERROR && (srcDisList[cou - 1] - srcDisList[cou - 2]) > DISMAXERROR)
            {
                return cou - 1;
            }
            //找不到相同的则返回-1
            return -1;
            //throw new NotImplementedException();
        }
        /// <summary>
        /// 找到一条相同的边，这条边的一个节点是key
        /// </summary>
        /// <param name="desDis"></param>
        /// <param name="key"></param>
        /// <param name="srcDisList"></param>
        /// <returns>返回距离的index，没有则返回-1</returns>
        private static int FindASameDis(double desDis, int key, List<double> srcDisList, List<KeyValuePair<int,int>> srcDisIndexList)
        {
            var srcKeyDisList = new List<double>();
            var srcKeyIndexList = new List<int>();
            for (int i = 0; i < srcDisIndexList.Count; i++)
            {
                var item = srcDisIndexList[i];
                if (item.Value == key || item.Key == key)
                {
                    srcKeyDisList.Add(srcDisList[i]);
                    srcKeyIndexList.Add(i);
                }
            }
            if (Math.Abs(srcKeyDisList[0] - desDis) < DISMINERROR && (srcKeyDisList[1] - srcKeyDisList[0]) > DISMAXERROR)
            {
                return srcKeyIndexList[0];
            }
            int cou = srcKeyDisList.Count;
            for (int i = 1; i < cou - 1; i++)
            {
                if (Math.Abs(srcKeyDisList[i] - desDis) < DISMINERROR &&
                    (srcKeyDisList[i + 1] - srcKeyDisList[i]) > DISMAXERROR &&
                    (srcKeyDisList[i] - srcKeyDisList[i - 1]) > DISMAXERROR)
                {
                    return srcKeyIndexList[i];
                }
            }
            if (Math.Abs(srcKeyDisList[cou - 1] - desDis) < DISMINERROR && (srcKeyDisList[cou - 1] - srcKeyDisList[cou - 2]) > DISMAXERROR)
            {
                return srcKeyIndexList[cou - 1];
            }
            //找不到相同的则返回-1
            return -1;
            throw new NotImplementedException();
        }
        /// <summary>
        /// 找有没有以key为起点的相似的边
        /// </summary>
        /// <param name="key"></param>
        /// <param name="value"></param>
        /// <param name="desDisList"></param>
        /// <param name="desDisIndexList"></param>
        /// <returns>没有返回true，有返回false</returns>
        private static bool haveNoSameDist(int key, int value, List<double> desDisList, List<KeyValuePair<int,int>> desDisIndexList)
        {
            int desIndex = -1;
            double desDis = -1;
            var desKeyDisList = new List<double>();
            for (int i = 0; i < desDisIndexList.Count; i++)
            {
                var item = desDisIndexList[i];
                if (item.Key == key || item.Value == key)
                {
                    desKeyDisList.Add(desDisList[i]);
                    if (item.Key + item.Value == key + value)
                    {
                        desIndex = desKeyDisList.Count-1;
                        desDis = desKeyDisList[desIndex];
                    }
                }
            }
            if (desIndex == desKeyDisList.Count - 1)
            {
                if (desDis - desKeyDisList[desIndex - 1] > DISMAXERROR)
                {
                    return true;
                }
                return false;
            }
            if (desIndex == 0)
            {
                if (desKeyDisList[1] - desDis > DISMAXERROR)
                {
                    return true;
                }
                return false;
            }
            if ((desKeyDisList[desIndex + 1] - desDis > DISMAXERROR && desDis - desKeyDisList[desIndex - 1] > DISMAXERROR))
            {
                return true;
            }
            return false;
        }
        private static bool NoInListValue(int no, List<KeyValuePair<int, int>> pairs)
        {
            for (int i = 0; i < pairs.Count; i++)
            {
                if (pairs[i].Value == no)
                {
                    return true;
                }
            }
            return false;
        }
        private static bool NoInListKey(int no, List<KeyValuePair<int, int>> pairs)
        {
            for (int i = 0; i < pairs.Count; i++)
            {
                if (pairs[i].Key == no)
                {
                    return true;
                }
            }
            return false;
            //throw new NotImplementedException();
        }
        //private static bool FindAPair(ref List<KeyValuePair<int,int>> pairs, ref List<Point> srcList, ref List<Point> desList, double disError)
        //{
        //    KeyValuePair<int,int> pp1 = pairs[0];
        //    List<int> srcIndexs = new List<int>();
        //    List<int> desIndexs = new List<int>();
        //    bool flag = false;
        //    for (int i = 0; i < srcList.Count; i++)
        //    {
        //        for (int m = 0; m < desList.Count; m++)
        //        {
        //            if (Math.Abs(pp1.SrcPoint.Distance(srcList[i]) - Math.Abs(pp1.DesPoint.Distance(desList[m]))) < disError)
        //            {
        //                srcIndexs.Add(i);
        //                desIndexs.Add(m);
        //            }
        //        }
        //    }


        //    for (int n = 0; n < srcIndexs.Count; n++)
        //    {
        //        if (ReCheckPair(pairs, srcList[srcIndexs[n]], desList[desIndexs[n]], disError))
        //        {
        //            pairs.Add(new PointPair(srcList[srcIndexs[n]], desList[desIndexs[n]]));
        //            srcList.Remove(srcList[srcIndexs[n]]);
        //            desList.Remove(desList[desIndexs[n]]);
        //            flag = true;
        //            break;
        //        }
        //    }

        //    return flag;
        //}        
    }
}
